home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Turnbull China Bikeride
/
Turnbull China Bikeride - Disc 1.iso
/
ARGONET
/
PD
/
MATHS
/
RLAB
/
RLAB125.ZIP
/
!RLaB
/
examples
/
lqrtest
< prev
next >
Wrap
Text File
|
1995-05-20
|
2KB
|
79 lines
printf( "Starting the lqr test...\n" );
rfile ode23
lqr = function( a, b, q, r )
{
m = size(a)[1]; n = size(a)[2];
mb = size(b)[1]; nb = size(b)[2];
mq = size(q)[1]; nq = size(q)[2];
if ( m != mq || n != nq )
{
fprintf( "stderr", "A and Q must be the same size.\n" );
quit
}
mr = size(r)[1]; nr = size(r)[2];
if ( mr != nr || nb != mr )
{
fprintf( "stderr", "B and R must be consistent.\n" );
quit
}
nn = zeros( m, nb );
// Start eigenvector decomposition by finding eigenvectors of Hamiltonian:
e = eig( [ a, solve(r',b')'*b'; q, -a' ] );
v = e.vec; d = e.val;
index = sort( real( d ) ).ind;
d = real( d[ index ] );
if ( !( d[n] < 0 && d[n+1] > 0 ) )
{
fprintf( "stderr", "Can't order eigenvalues.\n" );
quit
}
chi = v[ 1:n; index[1:n] ];
lambda = v[ (n+1):(2*n); index[1:n] ];
s = -real(solve(chi',lambda')');
k = solve( r, nn'+b'*s );
return << k=k; s=s >>;
};
// Now run a little test problem.
k = 1; m = 1; c = .1;
a = [ 0, 1, 0, 0; -k/m, -c/m, k/m, c/m; 0, 0, 0, 1; k/m, c/m, -k/m, -c/m ];
b = [ 0; 1/m; 0; 0 ];
qxx = diag( [0, 0, 100, 0] );
ruu = [1];
K = lqr( a, b, qxx, ruu ).k;
dot = function( t, x )
{
global (a, b, K)
return (a-b*K)*x + b*K*([1,0,1,0]')
};
x = ode( dot, 0, 15, [0,0,0,0]',.01, 1.e-5, 1.e-5);
m = maxi( x[;2] );
if ( (abs( x[m;2] - 1.195 ) > 0.001) || ...
any (abs( x[x.nr;2,4] - 1 ) > 0.001) ) {
printf( "...failed.\a\n" );
else
plgrid ();
pltitle ("LQR Test Results");
plot (x);
printf( "...passed.\n" );
}